/*
 * FifteenPuzzleNodeA.cpp
 *
 *  Created on: 22/05/2011
 *      Author: frandres
 */

#include "FifteenPuzzleNodeA.h"

FifteenPuzzleNodeA::FifteenPuzzleNodeA( )
{
	setHeapIndex(UINT_MAX);
}

FifteenPuzzleNodeA::FifteenPuzzleNodeA(int board[], int movFathers, int h, int g)
{
	setPosiciones(board);
	setH(h);
	setG(g);
	setFatherMovements(movFathers);
	setHeapIndex(UINT_MAX);
}


FifteenPuzzleNodeA::~FifteenPuzzleNodeA()
{

}

void FifteenPuzzleNodeA::setG(int newG)
{
	unsigned char g = toBinary(newG);
	dBoard[2] = (dBoard[2] & 0b11000000) | (g >> 1);
	dBoard[3] = (dBoard[3] & 0b01111111) | ((g & 0b00000001) << 7);

}


int FifteenPuzzleNodeA::getG()
{
	return (((dBoard[2] & 0b00111111) << 1) | (dBoard[3] >>7));
}

void FifteenPuzzleNodeA::setH(int newH)
{
	dBoard[3] = (dBoard[3] & 0b10000000) | toBinary(newH);
}

int FifteenPuzzleNodeA::getH()
{
	return (dBoard[3] & 0b01111111);
}

int FifteenPuzzleNodeA::getF()
{
	return getH()+getG();
}

void FifteenPuzzleNodeA::setHeapIndex(unsigned int newHeapIndex)
{
	heapIndex = newHeapIndex;
}

unsigned int FifteenPuzzleNodeA::getHeapIndex()
{
	return heapIndex;
}

int FifteenPuzzleNodeA::getFatherMovements()
{

	return (dBoard[2] >> 6);
}

void FifteenPuzzleNodeA::setFatherMovements(int movement)
{
	dBoard[2]= (dBoard[2]&0b00111111) | (toBinary(movement) <<6);
}

void FifteenPuzzleNodeA::setPosiciones(int newBoard[])
{

	int i,j;

	int aux[16];
	int invBoard [16];

	j = 0;
	for (i = 0; i<16; i++)
	{
		if (newBoard[i]>=12)
			invBoard[newBoard[i]] = i;
		else
			invBoard[newBoard[i]] = j++;
	}


	for (i = 0; i<12; i++)
		aux[invBoard[i]] = i;

	dBoard[0] = toBinary(invBoard[12]) <<4 ;
	dBoard[0] = this->dBoard[0] | toBinary(invBoard[13]);
	dBoard[1] = toBinary(invBoard[14]) << 4;
	dBoard[1] = this->dBoard[1] | toBinary(invBoard[15]);

	permutationRank= rrankPerm(12,invBoard,aux);
}

void FifteenPuzzleNodeA::getBoard(int board[])
{
	int i,j;

	int invBoard [16];
	int aux [12];

	for (i = 0; i<12; i++){
		invBoard[i] = i;
		board[i]=-1;
	}

	for (i = 12; i<16; i++){
		invBoard[i] = getPosicion(i);
		board[i]=-1;
	}

	unrankPerm(12,permutationRank,invBoard);

	/*cout << "\nLa inversa no reducida es: ";
	for (i = 0; i<16; i++)
			cout << invBoard[i] << " ";*/

	for (i = 0; i<12; i++)
		aux[invBoard[i]] = i;


	for (i = 12; i<16; i++)
		board[invBoard[i]] = i;


	j = 0;
	for (i = 0; i<16; i++){
		if (board[i]<0)
			board[i] = aux[j++];
	}
/*
 * for (i = 0; i<16; i++)
		cout << invBoard[i] << " ";
 */
}

void FifteenPuzzleNodeA::getInvBoard(int invBoard[])
{

	int aux[16];
	getBoard(aux);
	for (int i = 0; i<16;i++){
		invBoard[aux[i]]=i;
	}

}
/* Convert a number less than 81 to its binary form,
 * returning it in a char
 */
 unsigned char FifteenPuzzleNodeA::toBinaryArray [128] =
{0b00000000, 0b00000001, 0b00000010, 0b00000011, 0b00000100, 0b00000101, 0b00000110, 0b00000111,
 0b00001000, 0b00001001, 0b00001010, 0b00001011, 0b00001100, 0b00001101, 0b00001110, 0b00001111,
 0b00010000, 0b00010001, 0b00010010, 0b00010011, 0b00010100, 0b00010101, 0b00010110, 0b00010111,
 0b00011000, 0b00011001, 0b00011010, 0b00011011, 0b00011100, 0b00011101, 0b00011110, 0b00011111,
 0b00100000, 0b00100001, 0b00100010, 0b00100011, 0b00100100, 0b00100101, 0b00100110, 0b00100111,
 0b00101000, 0b00101001, 0b00101010, 0b00101011, 0b00101100, 0b00101101, 0b00101110, 0b00101111,
 0b00110000, 0b00110001, 0b00110010, 0b00110011, 0b00110100, 0b00110101, 0b00110110, 0b00110111,
 0b00111000, 0b00111001, 0b00111010, 0b00111011, 0b00111100, 0b00111101, 0b00111110, 0b00111111,
 0b01000000, 0b01000001, 0b01000010, 0b01000011, 0b01000100, 0b01000101, 0b01000110, 0b01000111,
 0b01001000, 0b01001001, 0b01001010, 0b01001011, 0b01001100, 0b01001101, 0b01001110, 0b01001111,
 0b01010000, 0b01010001, 0b01010010, 0b01010011, 0b01010100, 0b01010101, 0b01010110, 0b01010111,
 0b01011000, 0b01011001, 0b01011010, 0b01011011, 0b01011100, 0b01011101, 0b01011110, 0b01011111,
 0b01100000, 0b01100001, 0b01100010, 0b01100011, 0b01100100, 0b01100101, 0b01100110, 0b01100111,
 0b01101000, 0b01101001, 0b01101010, 0b01101011, 0b01101100, 0b01101101, 0b01101110, 0b01101111,
 0b01110000, 0b01110001, 0b01110010, 0b01110011, 0b01110100, 0b01110101, 0b01110110, 0b01110111,
 0b01111000, 0b01111001, 0b01111010, 0b01111011, 0b01111100, 0b01111101, 0b01111110, 0b01111111};

inline unsigned char FifteenPuzzleNodeA::toBinary(int numero)
{
	return toBinaryArray[numero];
}


int FifteenPuzzleNodeA::getPosicion(int numero)
{

	switch (numero){
	case 12:
		return dBoard[0]>>4;
	case 13:
		return dBoard[0]&0xF;
	case 14:
		return dBoard[1]>>4;
	case 15:
		return dBoard[1]&0xF;
	}

	return -1;

}

void FifteenPuzzleNodeA::getFather(int newBoard[]){

	int board[16];
	getBoard(board);
	int pos0 = 0;

	for (int i =0; i<16;i++){
		newBoard[i] = board[i];
		if (board[i]==0)
			pos0= i;
	}

	switch (getFatherMovements()) {
		case MOVE_DOWN:
			swapPermutation(newBoard,pos0,pos0+4);
			break;
		case MOVE_UP:
			swapPermutation(newBoard,pos0,pos0-4);
			break;
		case MOVE_LEFT:
			swapPermutation(newBoard,pos0,pos0-1);
			break;
		case MOVE_RIGHT:
			swapPermutation(newBoard,pos0,pos0+1);
			break;
		default:
			break;
	}
}
vector<FifteenPuzzleNodeA *> FifteenPuzzleNodeA::getSuccesors(Manhattan15Puzzle *h)
{
	vector<FifteenPuzzleNodeA *>  successors;
	int board[16];
	int g = getG()+1;
	getBoard(board);
	int pos0 = -1;

	for (int i =0; i<16;i++)
		if (board[i]==0)
		{
			pos0= i;
			break;
		}

	if (pos0 <12) // El numero se puede mover hacia arriba.
	{
		swapPermutation(board,pos0,pos0+4);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_UP,
				h->calculateHeuristic(board[pos0], pos0+4, MOVE_UP, getH()),g));
		swapPermutation(board,pos0,pos0+4);
	}

	if (pos0 > 3) // El numero se puede mover hacia abajo
	{
		swapPermutation(board,pos0,pos0-4);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_DOWN,
				h->calculateHeuristic(board[pos0], pos0-4,MOVE_DOWN, getH()),g));

		swapPermutation(board,pos0,pos0-4);
	}

	if (pos0%4 != 3) // El numero se puede mover hacia la izquierda
	{
		swapPermutation(board,pos0,pos0+1);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_LEFT,
				h->calculateHeuristic(board[pos0], pos0+1, MOVE_LEFT, getH()),g));

		swapPermutation(board,pos0,pos0+1);
	}

	if (pos0%4 != 0) // El numero se puede mover hacia la derecha
	{
		swapPermutation(board,pos0,pos0-1);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_RIGHT,
				h->calculateHeuristic(board[pos0], pos0-1, MOVE_RIGHT, getH()),g));

		swapPermutation(board,pos0,pos0-1);
	}

	return successors;
}

vector<FifteenPuzzleNodeA *> FifteenPuzzleNodeA::getSuccesors(PDBHeuristic *h1, PDBHeuristic *h2, PDBHeuristic *h3)
{
	vector<FifteenPuzzleNodeA *>  successors;
	int invBoard[16];
	int board[16];
	int g = getG()+1;
	int pos0 = -1;

	getBoard(board);

	for (int i =0; i<16;i++)
		invBoard[board[i]] = i;

	pos0 = invBoard[0];

	if (pos0 <12) // Se puede mover hacia arriba
	{
		swapPermutation(board,pos0,pos0+4);
		swapPermutation(invBoard, board[pos0], board[pos0+4]);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_UP,
				h1->calculateHeuristic(invBoard) + h2->calculateHeuristic(invBoard) + h3->calculateHeuristic(invBoard),g));

		swapPermutation(board,pos0,pos0+4);
		swapPermutation(invBoard, board[pos0], board[pos0+4]);
	}

	if (pos0 > 3) // Se puede mover hacia abajo
	{
		swapPermutation(board,pos0,pos0-4);
		swapPermutation(invBoard, board[pos0], board[pos0-4]);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_DOWN,
				h1->calculateHeuristic(invBoard) + h2->calculateHeuristic(invBoard) + h3->calculateHeuristic(invBoard),g));

		swapPermutation(board,pos0,pos0-4);
		swapPermutation(invBoard, board[pos0], board[pos0-4]);
	}

	if (pos0%4 != 3) // Se puede mover hacia la izquierda
	{
		swapPermutation(board,pos0,pos0+1);
		swapPermutation(invBoard, board[pos0], board[pos0+1]);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_LEFT,
				h1->calculateHeuristic(invBoard) + h2->calculateHeuristic(invBoard) + h3->calculateHeuristic(invBoard),g));

		swapPermutation(board,pos0,pos0+1);
		swapPermutation(invBoard, board[pos0], board[pos0+1]);
	}

	if (pos0%4 != 0) // Se puede mover hacia la derecha
	{
		swapPermutation(board,pos0,pos0-1);
		swapPermutation(invBoard, board[pos0], board[pos0-1]);

		successors.push_back(new FifteenPuzzleNodeA (board,MOVE_LEFT,
				h1->calculateHeuristic(invBoard) + h2->calculateHeuristic(invBoard) + h3->calculateHeuristic(invBoard),g));

		swapPermutation(board,pos0,pos0-1);
		swapPermutation(invBoard, board[pos0], board[pos0-1]);
	}

	return successors;
}

vector<FifteenPuzzleNodeA> FifteenPuzzleNodeA::getSuccesors(int board[], int invBoard[], PDBHeuristic *h1, int dh)
{

	vector<FifteenPuzzleNodeA>  successors;
	FifteenPuzzleNodeA Node;
	int pos0 = -1;
	int pn = 0;

	pos0 = invBoard[0];
	Node.setG(0);

	if (pos0 <12) // Se puede mover hacia arriba
	{
		swapPermutation(board,pos0,pos0+4);
		swapPermutation(invBoard, board[pos0], board[pos0+4]);

		Node.setPosiciones(board);
		pn = h1->getRank(invBoard);

		if(h1->H[pn] == 81)
		{
			h1->H[pn] = dh + (!h1->X[board[pos0]] ? 1 : 0);
			Node.setH(h1->H[pn]);
			successors.push_back(Node);
		}

		swapPermutation(board,pos0,pos0+4);
		swapPermutation(invBoard, board[pos0], board[pos0+4]);
	}

	if (pos0 > 3) // Se puede mover hacia abajo
	{
		swapPermutation(board,pos0,pos0-4);
		swapPermutation(invBoard, board[pos0], board[pos0-4]);

		Node.setPosiciones(board);
		pn = h1->getRank(invBoard);

		if(h1->H[pn] == 81)
		{
			h1->H[pn] = dh + (!h1->X[board[pos0]] ? 1 : 0);
			Node.setH(h1->H[pn]);
			successors.push_back(Node);
		}

		swapPermutation(board,pos0,pos0-4);
		swapPermutation(invBoard, board[pos0], board[pos0-4]);
	}

	if (pos0%4 != 3) // Se puede mover hacia la izquierda
	{
		swapPermutation(board,pos0,pos0+1);
		swapPermutation(invBoard, board[pos0], board[pos0+1]);

		Node.setPosiciones(board);
		pn = h1->getRank(invBoard);

		if(h1->H[pn] == 81)
		{
			h1->H[pn] = dh + (!h1->X[board[pos0]] ? 1 : 0);
			Node.setH(h1->H[pn]);
			successors.push_back(Node);
		}

		swapPermutation(board,pos0,pos0+1);
		swapPermutation(invBoard, board[pos0], board[pos0+1]);
	}

	if (pos0%4 != 0) // Se puede mover hacia la derecha
	{
		swapPermutation(board,pos0,pos0-1);
		swapPermutation(invBoard, board[pos0], board[pos0-1]);

		Node.setPosiciones(board);
		pn = h1->getRank(invBoard);

		if(h1->H[pn] == 81)
		{
			h1->H[pn] = dh + (!h1->X[board[pos0]] ? 1 : 0);
			Node.setH(h1->H[pn]);
			successors.push_back(Node);
		}

		swapPermutation(board,pos0,pos0-1);
		swapPermutation(invBoard, board[pos0], board[pos0-1]);
	}

	return successors;

}



void FifteenPuzzleNodeA::unrankPerm(int n, int r, int * perm)
{
	if (n > 0)
	{
		swapPermutation(perm,n-1,r%n);
		unrankPerm(n-1,r/n,perm);
	}
}

unsigned int FifteenPuzzleNodeA::rrankPerm(int n, int perm [] , int invPerm [])
{

	if (n==1)
		return 0;

	int s = perm [n-1];
	swapPermutation(perm,n-1,invPerm[n-1]);
	swapPermutation(invPerm,s,n-1);
	return (s+n*rrankPerm(n-1,perm,invPerm));

}

void FifteenPuzzleNodeA:: swapPermutation(int perm [], int pos1, int pos2)
{
	int temp = perm[pos1];
	perm[pos1] = perm[pos2];
	perm[pos2] = temp;
}

void FifteenPuzzleNodeA::printBoard(){
	int board[16];
	getBoard(board);
	int i,j;
	for (i = 0; i<16; i+=4){
		for (j = 0;j<4;j++){
			if (board[i+j]<10){
				cout <<" "<<board[i+j]<<" ";
			}else
				cout << board[i+j]<<" ";
		}
		cout << "\n";
	}
}
